{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "5ee3b8fc",
   "metadata": {},
   "source": [
    "# Robotics, Vision & Control 3e: for Python\n",
    "## Chapter 2: Representing position & orientation\n",
    "\n",
    "Copyright (c) 2021- Peter Corke"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9fa014e9",
   "metadata": {},
   "outputs": [],
   "source": [
    "try:\n",
    "    from google.colab import output\n",
    "    print('Running on CoLab')\n",
    "    output.enable_custom_widget_manager()\n",
    "    !pip install ipympl\n",
    "    !pip install spatialmath-python\n",
    "    COLAB = True\n",
    "    SWIFT = False\n",
    "except ModuleNotFoundError:\n",
    "    COLAB = False\n",
    "    SWIFT = False\n",
    "from IPython.core.interactiveshell import InteractiveShell\n",
    "InteractiveShell.ast_node_interactivity = \"last_expr_or_assign\"\n",
    "from IPython.display import HTML\n",
    "\n",
    "# standard imports\n",
    "import numpy as np\n",
    "from scipy import linalg\n",
    "%matplotlib widget\n",
    "import matplotlib.pyplot as plt\n",
    "import math\n",
    "from math import pi\n",
    "np.set_printoptions(\n",
    "    linewidth=120, formatter={\n",
    "        'float': lambda x: f\"{0:8.4g}\" if abs(x) < 1e-10 else f\"{x:8.4g}\"})\n",
    "np.random.seed(0)\n",
    "from spatialmath import *\n",
    "from spatialmath.base import *"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "b1de4c86",
   "metadata": {},
   "source": [
    "There are some minor code changes compared to the book. These are to support \n",
    "the Matplotlib widget (ipympl) backend.  This allows 3D plots to be rotated\n",
    "so the changes are worthwhile."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e8db33c6",
   "metadata": {},
   "source": [
    "# 2.1 Foundations\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "470988dd",
   "metadata": {},
   "source": [
    "# 2.2 Working in Two Dimensions (2D)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a09d2f95",
   "metadata": {},
   "source": [
    "## 2.2.1 Orientation in Two Dimensions\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9efe5df0",
   "metadata": {},
   "source": [
    "### 2.2.1.1 2D Rotation Matrix\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c5483e87",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = rot2(0.3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "64540c28",
   "metadata": {},
   "outputs": [],
   "source": [
    "plotvol2(new=True)  # for matplotlib/widget\n",
    "trplot2(R);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5af45966",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.linalg.det(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c16e841d",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.linalg.det(R @ R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e310167d",
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import Symbol, Matrix, simplify, pprint\n",
    "theta = Symbol('theta')\n",
    "R = Matrix(rot2(theta))  # convert to SymPy matrix"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7258a4db",
   "metadata": {},
   "outputs": [],
   "source": [
    "simplify(R * R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3b01396b",
   "metadata": {},
   "outputs": [],
   "source": [
    "R.det()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e67d0f7c",
   "metadata": {},
   "outputs": [],
   "source": [
    "R.det().simplify()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9b39fceb",
   "metadata": {},
   "source": [
    "### 2.2.1.2 Matrix Exponential for Rotation\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "31f1d102",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = rot2(0.3);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f19d040c",
   "metadata": {},
   "outputs": [],
   "source": [
    "L = linalg.logm(R)  # using linalg package of SciPy"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a9a341f8",
   "metadata": {},
   "outputs": [],
   "source": [
    "S = vex(L)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "77731ef6",
   "metadata": {},
   "outputs": [],
   "source": [
    "X = skew(2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "86db1b29",
   "metadata": {},
   "outputs": [],
   "source": [
    "vex(X)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3508ab12",
   "metadata": {},
   "outputs": [],
   "source": [
    "linalg.expm(L)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cec4db4a",
   "metadata": {},
   "outputs": [],
   "source": [
    "linalg.expm(skew(S))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "87688725",
   "metadata": {},
   "source": [
    "## 2.2.2 Pose in Two Dimensions\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "055de8ed",
   "metadata": {},
   "source": [
    "### 2.2.2.1 2D Homogeneous Transformation Matrix\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b018f9a7",
   "metadata": {},
   "outputs": [],
   "source": [
    "rot2(0.3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "909d6608",
   "metadata": {},
   "outputs": [],
   "source": [
    "trot2(0.3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7319f76a",
   "metadata": {},
   "outputs": [],
   "source": [
    "TA = transl2(1, 2) @ trot2(30, \"deg\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "79d9175b",
   "metadata": {},
   "outputs": [],
   "source": [
    "plotvol2([0, 5], new=True); # new plot with both axes from 0 to 5\n",
    "trplot2(TA, frame=\"A\", color=\"b\");\n",
    "T0 = transl2(0, 0);\n",
    "trplot2(T0, frame=\"0\", color=\"k\");  # reference frame\n",
    "TB = transl2(2, 1)\n",
    "trplot2(TB, frame=\"B\", color=\"r\");\n",
    "TAB = TA @ TB\n",
    "trplot2(TAB, frame=\"AB\", color=\"g\");\n",
    "TBA = TB @ TA;\n",
    "trplot2(TBA, frame=\"BA\", color=\"c\");\n",
    "P = np.array([3, 2]);\n",
    "plot_point(P, \"ko\", label=\"P\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "edcaf26b",
   "metadata": {},
   "outputs": [],
   "source": [
    "print(TA)\n",
    "print(P)\n",
    "np.linalg.inv(TA) @ np.hstack([P, 1])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5c994de2",
   "metadata": {},
   "outputs": [],
   "source": [
    "h2e(np.linalg.inv(TA) @ e2h(P))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2d49d780",
   "metadata": {},
   "outputs": [],
   "source": [
    "homtrans(np.linalg.inv(TA), P)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "84d879f8",
   "metadata": {},
   "source": [
    "### 2.2.2.2 Rotating a Coordinate Frame\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3bfe12ca",
   "metadata": {},
   "outputs": [],
   "source": [
    "plotvol2([-5, 4, -1, 5], new=True);  # for matplotlib/widget\n",
    "T0 = transl2(0, 0);\n",
    "trplot2(T0, frame=\"0\", color=\"k\");\n",
    "TX = transl2(2, 3);\n",
    "trplot2(TX, frame=\"X\", color=\"b\");\n",
    "TR = trot2(2);\n",
    "trplot2(TR @ TX, framelabel=\"RX\", color=\"g\");\n",
    "trplot2(TX @ TR, framelabel=\"XR\", color=\"g\");\n",
    "C = np.array([3, 2]);\n",
    "plot_point(C, \"ko\", text=\"C\");\n",
    "TC = transl2(C) @ TR @ transl2(-C)\n",
    "trplot2(TC @ TX, framelabel=\"XC\", color=\"r\");"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cd1b5baa",
   "metadata": {},
   "source": [
    "### 2.2.2.3 Matrix exponential for Pose\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ed8e7626",
   "metadata": {},
   "outputs": [],
   "source": [
    "L = linalg.logm(TC)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "134d047a",
   "metadata": {},
   "outputs": [],
   "source": [
    "S = vexa(L)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "bcef0ac8",
   "metadata": {},
   "outputs": [],
   "source": [
    "linalg.expm(skewa(S))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8c3345ba",
   "metadata": {},
   "outputs": [],
   "source": [
    "X = skewa([1, 2, 3])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b0571db2",
   "metadata": {},
   "outputs": [],
   "source": [
    "vexa(X)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1c53c8fb",
   "metadata": {},
   "source": [
    "### 2.2.2.4 2D Twists\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5e10e01c",
   "metadata": {},
   "outputs": [],
   "source": [
    "S = Twist2.UnitRevolute(C)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5aff09e3",
   "metadata": {},
   "outputs": [],
   "source": [
    "linalg.expm(skewa(2 * S.S))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "75b4a37f",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.exp(2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1d759daa",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.pole"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3a201e7a",
   "metadata": {},
   "outputs": [],
   "source": [
    "S = Twist2.UnitPrismatic([0, 1])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9f45a848",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.exp(2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "979ff0aa",
   "metadata": {},
   "outputs": [],
   "source": [
    "T = transl2(3, 4) @ trot2(0.5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f1387dec",
   "metadata": {},
   "outputs": [],
   "source": [
    "S = Twist2(T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "32bc8c36",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.w"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c40397de",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.pole"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ab1f355b",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.exp(1)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "26a02f07",
   "metadata": {},
   "source": [
    "# 2.3 Working in Three Dimensions (3D)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bf8037f4",
   "metadata": {},
   "source": [
    "## 2.3.1 Orientation in Three Dimensions\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "9bbb219d",
   "metadata": {},
   "source": [
    "### 2.3.1.1 3D Rotation Matrix\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1bfaea18",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = rotx(pi / 2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "424a2cec",
   "metadata": {},
   "outputs": [],
   "source": [
    "plotvol3(new=True)  # for matplotlib/widget\n",
    "trplot(R);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1ee4b844",
   "metadata": {},
   "outputs": [],
   "source": [
    "# tranimate(R)\n",
    "plotvol3(new=True)  # for matplotlib/widget\n",
    "HTML(tranimate(R, movie=True, dim=1.5))"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "ef8ac1d8",
   "metadata": {},
   "source": [
    "<span style=\"background-color:red; font-size:20pt\">NOTE</span>\n",
    "\n",
    "Robust, portable animation in Jupyter notebooks is challenging.  Here we use an option to `tranimate` that causes it to return the animation as a snippet of HTML5 which is then displayed\n",
    "```\n",
    "HTML(tranimate(R, movie=True))\n",
    "```\n",
    "If you wish to animate a coordinate frame from a regular Python script use the simpler syntax\n",
    "```\n",
    "tranimate(R)\n",
    "```\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "58c33ce8",
   "metadata": {},
   "outputs": [],
   "source": [
    "plotvol3(new=True)  # for matplotlib/widget\n",
    "trplot(R, anaglyph=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "30d32e00",
   "metadata": {},
   "outputs": [],
   "source": [
    "plotvol3(new=True)  # for matplotlib/widget\n",
    "HTML(tranimate(R, anaglyph=True, movie=True, dim=1.5))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8d1cb73c",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = rotx(pi / 2) @ roty(pi / 2)\n",
    "trplot(R);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2f9cbf31",
   "metadata": {},
   "outputs": [],
   "source": [
    "Ryx = roty(pi / 2) @ rotx(pi / 2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "3e6a5906",
   "metadata": {},
   "outputs": [],
   "source": [
    "plotvol3(new=True)  # for matplotlib/widget\n",
    "trplot(Ryx);"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "58c87b1b",
   "metadata": {},
   "source": [
    "### 2.3.1.2 Three-Angle Representations\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5e989fcb",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = rotz(0.1) @ roty(0.2) @ rotz(0.3);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cf1dd849",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = eul2r(0.1, 0.2, 0.3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "046b7a14",
   "metadata": {},
   "outputs": [],
   "source": [
    "gamma = tr2eul(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "92ee36eb",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = eul2r(0.1, -0.2, 0.3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fde5c3c0",
   "metadata": {},
   "outputs": [],
   "source": [
    "gamma = tr2eul(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "25f28dec",
   "metadata": {},
   "outputs": [],
   "source": [
    "eul2r(gamma)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fb21bb5b",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = eul2r(0.1, 0, 0.3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d662beb2",
   "metadata": {},
   "outputs": [],
   "source": [
    "tr2eul(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "77bd0546",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = rpy2r(0.1, 0.2, 0.3, order=\"zyx\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cafc941b",
   "metadata": {},
   "outputs": [],
   "source": [
    "gamma = tr2rpy(R, order=\"zyx\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9e0c0b74",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = rpy2r(0.1, 0.2, 0.3, order=\"xyz\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "43843ceb",
   "metadata": {},
   "outputs": [],
   "source": [
    "gamma = tr2rpy(R, order=\"xyz\")"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "941e7e1b",
   "metadata": {},
   "source": [
    "<span style=\"background-color:red; font-size:20pt\">NOTE</span>\n",
    "\n",
    "The next cell will launch an interactive tool (using the Swift visualizer) in a new browser tab.  Close the browser tab when you are done with it. \n",
    "\n",
    "You might also have to stop the cell from executing, by pressing the stop button for the cell. It may terminate with lots of errors, don't panic."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "81fe596f",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB or not SWIFT:\n",
    "    print(\"we can't run this demo from the Colab environment (yet)\")\n",
    "else:\n",
    "    %run -m tripleangledemo"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5659bd11",
   "metadata": {},
   "source": [
    "### 2.3.1.3 Singularities and Gimbal Lock\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "c34b143f",
   "metadata": {},
   "source": [
    "### 2.3.1.4 Two-Vector Representation\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d42e7aed",
   "metadata": {},
   "outputs": [],
   "source": [
    "a = [0, 0, -1]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5f471c77",
   "metadata": {},
   "outputs": [],
   "source": [
    "o = [1, 1, 0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b2ae1c1e",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = oa2r(o, a)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1e89da75",
   "metadata": {},
   "source": [
    "### 2.3.1.5 Rotation about an Arbitrary Vector\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2577da8c",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = rpy2r(0.1, 0.2, 0.3);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2be19376",
   "metadata": {},
   "outputs": [],
   "source": [
    "theta, v = tr2angvec(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9854152d",
   "metadata": {},
   "outputs": [],
   "source": [
    "theta"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fb457f07",
   "metadata": {},
   "outputs": [],
   "source": [
    "v"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a8e0dc79",
   "metadata": {},
   "outputs": [],
   "source": [
    "e, x = np.linalg.eig(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c86dd524",
   "metadata": {},
   "outputs": [],
   "source": [
    "e"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "28722551",
   "metadata": {},
   "outputs": [],
   "source": [
    "x"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "72187994",
   "metadata": {},
   "outputs": [],
   "source": [
    "theta = np.angle(e[0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a569a8f3",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = angvec2r(0.3, [1, 0, 0])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "eae0d700",
   "metadata": {},
   "source": [
    "### 2.3.1.6 Matrix Exponential for Rotation\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6951d4a6",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = rotx(0.3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "65881227",
   "metadata": {},
   "outputs": [],
   "source": [
    "L = linalg.logm(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ace7a108",
   "metadata": {},
   "outputs": [],
   "source": [
    "S = vex(L)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "2d6187e3",
   "metadata": {},
   "outputs": [],
   "source": [
    "L = trlog(R);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6fa59f7b",
   "metadata": {},
   "outputs": [],
   "source": [
    "linalg.expm(L)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "38638554",
   "metadata": {},
   "outputs": [],
   "source": [
    "trexp(L);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f45a04b9",
   "metadata": {},
   "outputs": [],
   "source": [
    "linalg.expm(skew(S))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6d77dbfd",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = rotx(0.3);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a263d94b",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = linalg.expm(0.3 * skew([1, 0, 0]));"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "70011333",
   "metadata": {},
   "outputs": [],
   "source": [
    "X = skew([1, 2, 3])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5444a763",
   "metadata": {},
   "outputs": [],
   "source": [
    "vex(X)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "87d0d0dc",
   "metadata": {},
   "source": [
    "### 2.3.1.7 Unit Quaternions\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "126b7bbf",
   "metadata": {},
   "outputs": [],
   "source": [
    "q = UnitQuaternion(rpy2r(0.1, 0.2, 0.3))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7542f730",
   "metadata": {},
   "outputs": [],
   "source": [
    "q = q * q;"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f7b183d5",
   "metadata": {},
   "outputs": [],
   "source": [
    "q.inv()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "844b0564",
   "metadata": {},
   "outputs": [],
   "source": [
    "q * q.inv()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "36709904",
   "metadata": {},
   "outputs": [],
   "source": [
    "q / q"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "53410f72",
   "metadata": {},
   "outputs": [],
   "source": [
    "q.R"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8f57ffb4",
   "metadata": {},
   "outputs": [],
   "source": [
    "q * [1, 0, 0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7e95c736",
   "metadata": {},
   "outputs": [],
   "source": [
    "plotvol3(new=True)  # for matplotlib/widget\n",
    "q.plot();"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "32ff989b",
   "metadata": {},
   "source": [
    "## 2.3.2 Pose in Three Dimensions\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1207ddf7",
   "metadata": {},
   "source": [
    "### 2.3.2.1 Homogeneous Transformation Matrix\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "220c3cae",
   "metadata": {},
   "outputs": [],
   "source": [
    "T = transl(2, 0, 0) @ trotx(pi / 2) @ transl(0, 1, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9766e56a",
   "metadata": {},
   "outputs": [],
   "source": [
    "plotvol3(new=True)  # for matplotlib/widget\n",
    "trplot(T);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c9fb9992",
   "metadata": {},
   "outputs": [],
   "source": [
    "t2r(T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "10aac3e0",
   "metadata": {},
   "outputs": [],
   "source": [
    "transl(T)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8af10b40",
   "metadata": {},
   "source": [
    "### 2.3.2.2 Matrix exponential for Pose\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7a45dbb9",
   "metadata": {},
   "outputs": [],
   "source": [
    "T = transl(2, 3, 4) @ trotx(0.3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4656e369",
   "metadata": {},
   "outputs": [],
   "source": [
    "L = linalg.logm(T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a9e11fdd",
   "metadata": {},
   "outputs": [],
   "source": [
    "S = vexa(L)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4e33da81",
   "metadata": {},
   "outputs": [],
   "source": [
    "linalg.expm(skewa(S))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6c150a70",
   "metadata": {},
   "outputs": [],
   "source": [
    "X = skewa([1, 2, 3, 4, 5, 6])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "905cfa13",
   "metadata": {},
   "outputs": [],
   "source": [
    "vexa(X)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d2509d69",
   "metadata": {},
   "source": [
    "### 2.3.2.3 3D Twists\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "34771835",
   "metadata": {},
   "outputs": [],
   "source": [
    "S = Twist3.UnitRevolute([1, 0, 0], [0, 0, 0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5be081af",
   "metadata": {},
   "outputs": [],
   "source": [
    "linalg.expm(0.3 * skewa(S.S));  # different to book, see §2.2.1.2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e46c22b7",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.exp(0.3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fde2289f",
   "metadata": {},
   "outputs": [],
   "source": [
    "S = Twist3.UnitRevolute([0, 0, 1], [2, 3, 2], 0.5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7d5e15c2",
   "metadata": {},
   "outputs": [],
   "source": [
    "X = transl(3, 4, -4);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ace2dd37",
   "metadata": {},
   "outputs": [],
   "source": [
    "for theta in np.arange(0, 15, 0.3):\n",
    "  trplot(S.exp(theta).A @ X, style=\"rviz\", width=2)\n",
    "\n",
    "L = S.line()\n",
    "L.plot('k:', linewidth=2);\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dcd011d0",
   "metadata": {},
   "outputs": [],
   "source": [
    "S = Twist3.UnitPrismatic([0, 1, 0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6f5aa456",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.exp(2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a6c9a477",
   "metadata": {},
   "outputs": [],
   "source": [
    "T = transl(1, 2, 3) @ eul2tr(0.3, 0.4, 0.5);\n",
    "S = Twist3(T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a54b9d44",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.w"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a4ecbeaa",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.pole"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5ecda422",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.pitch"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c3c03a18",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.theta"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "1c28aa8a",
   "metadata": {},
   "source": [
    "# 2.4 Advanced Topics\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "79abcd16",
   "metadata": {},
   "source": [
    "## 2.4.5 Distance Between Orientations\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9ba255b5",
   "metadata": {},
   "outputs": [],
   "source": [
    "UnitQuaternion.Rx(pi / 2).angdist(UnitQuaternion.Rz(-pi / 2))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7bed38da",
   "metadata": {},
   "source": [
    "## 2.4.6 Normalization\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1bfb56e9",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = np.eye(3,3);\n",
    "np.linalg.det(R) - 1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ad37d946",
   "metadata": {},
   "outputs": [],
   "source": [
    "for i in range(100):\n",
    "  R = R @ rpy2r(0.2, 0.3, 0.4);\n",
    "np.linalg.det(R) - 1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "50153ac8",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = trnorm(R);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9cc46fe4",
   "metadata": {},
   "outputs": [],
   "source": [
    "np.linalg.det(R) - 1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7d9bd6c9",
   "metadata": {},
   "outputs": [],
   "source": [
    "q = q.unit();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "499334cf",
   "metadata": {},
   "outputs": [],
   "source": [
    "# T = T1 @ T2\n",
    "# q = q1 @ q2"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "41c8ccdc",
   "metadata": {},
   "source": [
    "## 2.4.8 More About Twists\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "21bf10e0",
   "metadata": {},
   "outputs": [],
   "source": [
    "S = Twist3.UnitRevolute([1, 0, 0], [0, 0, 0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c5ece89c",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.S\n",
    "S.v\n",
    "S.w"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "707f6f6f",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.skewa()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f9407ee3",
   "metadata": {},
   "outputs": [],
   "source": [
    "trexp(0.3 * S.skewa())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "dafadf8b",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.exp(0.3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d7dc57d1",
   "metadata": {},
   "outputs": [],
   "source": [
    "S2 = S * S\n",
    "S2.printline(orient=\"angvec\", unit=\"rad\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "c87041f0",
   "metadata": {},
   "outputs": [],
   "source": [
    "line = S.line()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9ad8efc7",
   "metadata": {},
   "outputs": [],
   "source": [
    "plotvol3([-5, 5], new=True)  # setup volume in which to display the line\n",
    "line.plot(\"k:\", linewidth=2);"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "14b6cfe6",
   "metadata": {},
   "outputs": [],
   "source": [
    "T = transl(1, 2, 3) @ eul2tr(0.3, 0.4, 0.5);\n",
    "S = Twist3(T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e30bb7ff",
   "metadata": {},
   "outputs": [],
   "source": [
    "S / S.theta"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ca81e287",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.unit();"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "311428f1",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.exp(0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "72d0f3e3",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.exp(1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "97474ccb",
   "metadata": {},
   "outputs": [],
   "source": [
    "S.exp(0.5)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "17a75aef",
   "metadata": {},
   "source": [
    "<span style=\"background-color:red; font-size:20pt\">NOTE</span>\n",
    "\n",
    "The next cell will launch an interactive tool (using the Swift visualizer) in a new browser tab.  Close the browser tab when you are done with it. \n",
    "\n",
    "You might also have to stop the cell from executing, by pressing the stop button for the cell. It may terminate with lots of errors, don't panic."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ca6fcaee",
   "metadata": {},
   "outputs": [],
   "source": [
    "if COLAB or not SWIFT:\n",
    "    print(\"we can't run this demo from the Colab environment (yet)\")\n",
    "else:\n",
    "    %run -m twistdemo"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "5898e246",
   "metadata": {},
   "source": [
    "# 2.5 Using the Toolbox\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "97b30b70",
   "metadata": {},
   "outputs": [],
   "source": [
    "from spatialmath.base import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "24e5ddca",
   "metadata": {},
   "outputs": [],
   "source": [
    "from spatialmath import *"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "733dff8b",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = rotx(0.3)  # create SO(3) matrix as NumPy array\n",
    "type(R)\n",
    "R = SO3.Rx(0.3)  # create SO3 object\n",
    "type(R)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "4006df6a",
   "metadata": {},
   "outputs": [],
   "source": [
    "R.A"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "825b3479",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = SO3(rotx(0.3));                   # convert an SO(3) matrix\n",
    "R = SO3.Rz(0.3);                      # rotation about z-axis\n",
    "R = SO3.RPY(10, 20, 30, unit=\"deg\");  # from roll-pitch-yaw angles\n",
    "R = SO3.AngleAxis(0.3, (1, 0, 0));    # from angle and rotation axis\n",
    "R = SO3.EulerVec((0.3, 0, 0));        # from an Euler vector"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "1f5c5fa1",
   "metadata": {},
   "outputs": [],
   "source": [
    "R.rpy();        # convert to roll-pitch-yaw angles\n",
    "R.eul();        # convert to Euler angles\n",
    "R.printline();  # compact single-line print"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5684a982",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = SO3.RPY(10, 20, 30, unit=\"deg\");             # create an SO(3) rotation\n",
    "T = SE3.RPY(10, 20, 30, unit=\"deg\");             # create a purely rotational SE(3)\n",
    "S = Twist3.RPY(10, 20, 30, unit=\"deg\");          # create a purely rotational twist\n",
    "q = UnitQuaternion.RPY(10, 20, 30, unit=\"deg\");  # create a unit quaternion"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b112713d",
   "metadata": {},
   "outputs": [],
   "source": [
    "TA = SE2(1, 2) * SE2(30, unit=\"deg\");\n",
    "type(TA)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d8f4a5ef",
   "metadata": {},
   "outputs": [],
   "source": [
    "TA"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "ac480c83",
   "metadata": {},
   "outputs": [],
   "source": [
    "TA = SE2(1, 2, 30, unit=\"deg\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f77fb728",
   "metadata": {},
   "outputs": [],
   "source": [
    "TA.R\n",
    "TA.t"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "39a6e052",
   "metadata": {},
   "outputs": [],
   "source": [
    "plotvol2(new=True)  # for matplotlib/widget\n",
    "TA.plot(frame=\"A\", color=\"b\");"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f080b400",
   "metadata": {},
   "outputs": [],
   "source": [
    "TA.printline()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b168b518",
   "metadata": {},
   "outputs": [],
   "source": [
    "P = [3, 2];\n",
    "TA.inv() * P"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a9680b61",
   "metadata": {},
   "outputs": [],
   "source": [
    "R = SO3.Rx(np.linspace(0, 1, 5));\n",
    "len(R)\n",
    "R[3]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e9485386",
   "metadata": {},
   "outputs": [],
   "source": [
    "R * [1, 2, 3]"
   ]
  }
 ],
 "metadata": {
  "jupytext": {
   "cell_metadata_filter": "-all",
   "main_language": "python",
   "notebook_metadata_filter": "-all"
  },
  "kernelspec": {
   "display_name": "dev",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.9"
  },
  "vscode": {
   "interpreter": {
    "hash": "b7d6b0d76025b9176285a6442c3dd6dd39bcfe7241029b7898b7106bd5e9b472"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
